#include <cmath>
#include <string>
#include <GL/gl.h>
#include <GL/glu.h>

#include <SRL/Utils/Vector.h>
#include <SRL/Utils/Matrix.h>
#include <SRL/Utils/Extensions.h>
#include <SRL/Engine/Camera.h>
#include <SRL/Engine/Texture.h>
#include <SRL/Engine/Terrain.h>

namespace srl {

    TNode::TNode(){}
    
    ///////////////////////////////////////////////////////////////////////////
    // Init the node
    TNode::TNode(srl::TNode* pParent, int Direction, TerrainLOD* pRoot) {
        Parent = pParent;  
        Root = pRoot;
        Niveau = Parent->Niveau +1;
        
        for(int i = 0; i < 4; i++) {
            Child[i] = NULL;
            ASide[i] = false; }
            
        float Size = (Parent->Side[SRL_Top].x - Parent->Corner[SRL_TopLeft].x) /2;
        
        // Define the Center, to calculate others vectors latter
        switch (Direction) {
            case SRL_TopRight:
                Center.x = Parent->Center.x + Size;
                Center.z = Parent->Center.z + Size;
                break;
                
            case SRL_TopLeft:
                Center.x = Parent->Side[SRL_Left].x + Size;
                Center.z = Parent->Side[SRL_Left].z + Size;
                break;
                
            case SRL_BottomLeft:
                Center.x = Parent->Corner[SRL_BottomLeft].x + Size;
                Center.z = Parent->Corner[SRL_BottomLeft].z + Size;
                break;
                
            case SRL_BottomRight:
                Center.x = Parent->Side[SRL_Bottom].x + Size;
                Center.z = Parent->Side[SRL_Bottom].z + Size;
                break;
        }
        float a, b, c, d;
        a = Root->Datas[(int)ceil(Center.x) ][(int)ceil(Center.z) ];
        b = Root->Datas[(int)ceil(Center.x) ][(int)floor(Center.z)];
        c = Root->Datas[(int)floor(Center.x)][(int)ceil(Center.z) ];
        d = Root->Datas[(int)floor(Center.x)][(int)floor(Center.z)];
        Center.y = (a+b+c+d)/4;
        
        // Define corners
        Corner[SRL_TopRight].x = Center.x + Size;
        Corner[SRL_TopRight].y = 0;
        Corner[SRL_TopRight].z = Center.z + Size;
        
        Corner[SRL_BottomRight].x = Center.x + Size;
        Corner[SRL_BottomRight].y = 0;
        Corner[SRL_BottomRight].z = Center.z - Size;
        
        Corner[SRL_TopLeft].x = Center.x - Size;
        Corner[SRL_TopLeft].y = 0;
        Corner[SRL_TopLeft].z = Center.z + Size;
        
        Corner[SRL_BottomLeft].x = Center.x - Size;
        Corner[SRL_BottomLeft].y = 0;
        Corner[SRL_BottomLeft].z = Center.z - Size;
        
        switch (Direction) {
            case SRL_TopRight:
                Corner[SRL_BottomLeft].y = Parent->Center.y;
                Corner[SRL_BottomRight].y = Parent->Side[SRL_Right].y;
                Corner[SRL_TopLeft].y = Parent->Side[SRL_Top].y;
                Corner[SRL_TopRight].y = Parent->Corner[SRL_TopRight].y;
                break;
                
            case SRL_TopLeft:
                Corner[SRL_BottomRight].y = Parent->Center.y;
                Corner[SRL_BottomLeft].y = Parent->Side[SRL_Left].y;
                Corner[SRL_TopRight].y = Parent->Side[SRL_Top].y;
                Corner[SRL_TopLeft].y = Parent->Corner[SRL_TopLeft].y;
                break;
                
            case SRL_BottomLeft:
                Corner[SRL_TopRight].y = Parent->Center.y;
                Corner[SRL_TopLeft].y = Parent->Side[SRL_Left].y;
                Corner[SRL_BottomRight].y = Parent->Side[SRL_Bottom].y;
                Corner[SRL_BottomLeft].y = Parent->Corner[SRL_BottomLeft].y;
                break;
                
            case SRL_BottomRight:
                Corner[SRL_TopLeft].y = Parent->Center.y;
                Corner[SRL_TopRight].y = Parent->Side[SRL_Right].y;
                Corner[SRL_BottomLeft].y = Parent->Side[SRL_Bottom].y;
                Corner[SRL_BottomRight].y = Parent->Corner[SRL_BottomRight].y;
                break;
        }
        
        // Define sides
        int XP, XN, ZP, ZN;
        XP = (int)ceil(Center.x + Size);
        XN = (int)floor(Center.x - Size);
        ZP = (int)ceil(Center.z + Size);
        ZN = (int)floor(Center.z - Size);
        
        Side[SRL_Top].x = Center.x;
        Side[SRL_Top].y = Root->Datas[XP][ZP]+Root->Datas[XN][ZP]/2;
        Side[SRL_Top].z = Center.z + Size;
        
        Side[SRL_Bottom].x = Center.x;
        Side[SRL_Bottom].y = Root->Datas[XP][ZN]+Root->Datas[XN][ZN]/2;
        Side[SRL_Bottom].z = Center.z - Size;

        Side[SRL_Left].x = Center.x - Size;
        Side[SRL_Left].y = Root->Datas[XN][ZN]+Root->Datas[XN][ZP]/2;
        Side[SRL_Left].z = Center.z;
                
        Side[SRL_Right].x = Center.x + Size;
        Side[SRL_Right].y = Root->Datas[XP][ZN]+Root->Datas[XP][ZP]/2;
        Side[SRL_Right].z = Center.z;
        
        for(int i = 0; i < 4; i++) {
            // If the height is not directly an index in the heights array, then we find it by an interpolation beetween neightbours heights
            int x = (int)Side[i].x;
            int z = (int)Side[i].z;
            if((Root->Datas[x][z] - (int)Root->Datas[x][z]) > 0) {
                float a, b, c, d; 
                a = Root->Datas[(int)ceil(x) ][(int)ceil(z) ];
                b = Root->Datas[(int)ceil(x) ][(int)floor(z)];
                c = Root->Datas[(int)floor(x)][(int)ceil(z) ];
                d = Root->Datas[(int)floor(x)][(int)floor(z)];
                Side[i].y = (a+b+c+d)/4;
            }
            else {
                Side[i].y = Root->Datas[x][z];
            }
        }
        
        // Make children if necessary
        Init();
    }
    
    
    
    ///////////////////////////////////////////////////////////////////////////
    // Draw the node and his children
    void TNode::Draw() {
        // If it has 4 children, don't draw the node, just their children
        if(!( Child[SRL_TopLeft] != NULL && Child[SRL_TopRight] != NULL && Child[SRL_BottomLeft] != NULL && Child[SRL_BottomRight] != NULL )) {
            
            //We start with the center, to connect all the triangle make by this base.
            glBegin(GL_TRIANGLE_FAN);
            glMultiTexCoord2f(GL_TEXTURE0, Center.x/Root->Width, Center.z/Root->Height);
            glNormal3f(0, 1, 0);
            glVertex3f(Center.x, Center.y, Center.z);
            
            // Explore all children
            for(int i = 0; i < 4; i++) {
                
                // If there isn't a child here, we draw the corner
                if(Child[i] == NULL) {
                    glMultiTexCoord2f(GL_TEXTURE0, Corner[i].x/Root->Width, Corner[i].z/Root->Height);
                    glNormal3f(0, 1, 0);
                    glVertex3f(Corner[i].x, Corner[i].y, Corner[i].z);
                }
                
                // If any node has activate the side, then we draw it 
                if(ASide[i]) {
                    glMultiTexCoord2f(GL_TEXTURE0, Side[i].x/Root->Width, Side[i].z/Root->Height);
                    glNormal3f(0, 1, 0);
                    glVertex3f(Side[i].x, Side[i].y, Side[i].z);
                }
                // If there is a child after, we skip it by adding the center
                if(Child[ (i+1 > 3) ? 0 : i+1 ] != NULL) {
                    glMultiTexCoord2f(GL_TEXTURE0, Center.x/Root->Width, Center.z/Root->Height);
                    glNormal3f(0, 1, 0);
                    glVertex3f(Center.x, Center.y, Center.z);
                }
            }
            glMultiTexCoord2f(GL_TEXTURE0, Corner[SRL_TopRight].x/Root->Width, Corner[SRL_TopRight].z/Root->Height);
            glNormal3f(0, 1, 0);
            glVertex3f(Corner[SRL_TopRight].x, Corner[SRL_TopRight].y, Corner[SRL_TopRight].z);
            glEnd();
        }
        
        // Finally, we draw children
        for(int i = 0; i <4; i++) {
            if(Child[i]!=NULL)
            Child[i]->Draw();
        }
    }
    
    
    
    ///////////////////////////////////////////////////////////////////////////
    // Activate a child
    void TNode::CreateChild(int Direction) {
        // If the children doesn't exist
        if(Child[Direction] == NULL) {
            
            //Create the child
            Child[Direction] = new TNode(this, Direction, Root);
            
            // Activate the side
            ASide[Direction] = true;
            ASide[ (Direction-1 < 0) ? 3 : Direction-1 ] = true;
            
            // Activate adjacent sides
            TNode *Adj = Adjacent(Direction);
            if(Adj != NULL)
                Adj->ASide[ (Direction+2)%4 ] = true;
                
            Adj = Adjacent( (Direction-1 < 0) ? 3 : Direction-1 );
            if(Adj != NULL)
                Adj->ASide[ (Direction-1 < 0) ? 3 : Direction-1 ] = true;
        }
    }
    
    
    
    
    ///////////////////////////////////////////////////////////////////////////
    // Get the ajacent node of this node
    TNode* TNode::Adjacent(int Direction) {
        
    
        // Verify if we arn't on borders
        if( (Direction == SRL_Left && Corner[SRL_TopLeft].x == 0) ||
            (Direction == SRL_Right && Corner[SRL_BottomRight].x == Root->Width) ||
            (Direction == SRL_Top && Corner[SRL_TopLeft].z == Root->Height) ||
            (Direction == SRL_Bottom && Corner[SRL_BottomRight].z == 0)) {
                //printf("MATRIX ERROR : %i \n   %f  %f     %f  %f\n\n\n   %f  %f      %f  %f\n\n\n\n", Direction, Corner[SRL_TopLeft].x, Corner[SRL_TopLeft].z,   Corner[SRL_TopRight].x, Corner[SRL_TopRight].z, Corner[SRL_BottomLeft].x, Corner[SRL_BottomLeft].z, Corner[SRL_BottomRight].x, Corner[SRL_BottomRight].z);
                return NULL;
    }
        
        // We make a pointeur for the adjacent node.
        TNode* Adj = Root;
        
        // We get the position (topleft corner) of the adjacent node
        srl::Vector<float> AdjPos;
        float Size = (Parent->Corner[SRL_TopRight].x - Parent->Corner[SRL_TopLeft].x)/2;
        switch (Direction) {
            case SRL_Right:
                AdjPos.x = Side[SRL_Right].x - Size;
                AdjPos.z = Side[SRL_Right].z;
                break;
                
            case SRL_Left:
                AdjPos.x = Side[SRL_Left].x + Size;
                AdjPos.z = Side[SRL_Left].z;
                break;
                
            case SRL_Bottom:
                AdjPos.x = Side[SRL_Bottom].x;
                AdjPos.z = Side[SRL_Bottom].z + Size;
                break;
                
            case SRL_Top:
                AdjPos.x = Side[SRL_Top].x;
                AdjPos.z = Side[SRL_Top].z - Size;
                break;
        }
        
        // We get the correspondant corner and side
        int AdjCornerA = (Direction-1 < 0) ? 3 : Direction-1 ;
        int AdjCornerB = (AdjCornerA-1 < 0) ? 3 : AdjCornerA-1 ;
        
        int CurCornerA = Direction;
        int CurCornerB = (Direction+1 > 3) ? 0 : Direction+1 ;
        
        // While we havn't found the adjacent node
        while( Corner[CurCornerA] != Adj->Corner[AdjCornerA] && Corner[CurCornerB] != Adj->Corner[AdjCornerB] ) {

            
            // We search in witch child is the adjacent
            int DirectionNode;
            
            if( AdjPos.z < Adj->Center.x ) {       // Left
                if( AdjPos.y < Adj->Center.z ) {   // Bottom
                    DirectionNode = 2;
                } else {                                // Top
                    DirectionNode = 1;
                }
            }
            else  {                                     // Right
                if( AdjPos.z < Adj->Center.z ) {   // Bottom
                    DirectionNode = 3;
                } else {                                // Top
                    DirectionNode = 0;
                }
            }    
            
            // Attribute the new node to the adjacent pointer
            if(Adj->Child[DirectionNode] == NULL) 
                return NULL;
            else
                Adj = Adj->Child[DirectionNode];
        }
    
        // Return the adjacent founded
        return Adj;
    }
    
    
    
    ///////////////////////////////////////////////////////////////////////////
    // Init children's node if necessary
    void TNode::Init() {
        for(int i = 0; i < 4; i++) {
            float Diff = Differance(i);
            //printf("Niveau : %i \n   %f != 0 && %f < %f \n\n\n", Niveau, Diff, Diff, Root->HauteurMax);
            if( Diff != 0 && Diff < Root->HauteurMax && Niveau < 5)
                CreateChild(i);
           
        }
    }
    
    
    
    
    ////////////////////////////////////////////////////////////////////////////
    // Get the max differance in heights of a child
    float TNode::Differance(int Direction) {   
        
        // Get the start position (the bottom left corner) and the size
        srl::Vector<float> BottomLeft;
        srl::Vector<float> TopRight;
        float Size = Root->Width/((Niveau+1)*2);
        switch (Direction) {
            case SRL_TopLeft:
                BottomLeft.x = Side[SRL_Left].x;
                BottomLeft.z = Side[SRL_Left].z;
                break;
                
            case SRL_TopRight:
                BottomLeft.x = Center.x;
                BottomLeft.z = Center.z;
                break;
                
            case SRL_BottomLeft:
                BottomLeft.x = Corner[SRL_BottomLeft].x;
                BottomLeft.z = Corner[SRL_BottomLeft].z;
                break;
                
            case SRL_BottomRight:
                BottomLeft.x = Side[SRL_Bottom].x;
                BottomLeft.z = Side[SRL_Bottom].z;
                break;
        }
        TopRight = BottomLeft + Size;
        
        // Get the min and max values
        float min = 0; float max = 0;
        for(int x = (int)BottomLeft.x; x < (int)TopRight.x; x++) {
            for(int z = (int)BottomLeft.z; z < (int)TopRight.z; z++) {
                
                // Get if it's the max
                if(Root->Datas[x][z] > max)
                     max = Root->Datas[x][z];
                
                // Or the min
                if(Root->Datas[x][z] < min)
                     min = Root->Datas[x][z];
            }
        }
        // Return the difference between the max and min values
        return max-min;
    }
    
    
    
    
    
    
    ////////////////////////////////////////////////////////////////////////////
    // A special TNode element -> the Root
    
    TerrainLOD::TerrainLOD() : Width(50.0f), Height(50.0f) {
        Root = this;
        Parent = this;
        HauteurMax = 10.0f;
        Niveau = 0;
            
        for(int i = 0; i < 4; i++) {
            Child[i] = NULL;
            ASide[i] = false; }
        
        // Define center
        Center.x = Width/2;
        Center.z = Height/2;
        Center.y = 0;

        float Size = Width;
        
        for(int i=0;i<Size; i++)
            for(int j=0;j<Size; j++)
                Datas[i][j] = 0.0;
        
        /*Datas[24][24] = 1;
        Datas[24][25] = 1;
        Datas[24][26] = 1;
        
        Datas[25][24] = 1;
        Datas[25][25] = 2;
        Datas[25][26] = 1;
        
        Datas[25][24] = 1;
        Datas[25][25] = 1;
        Datas[25][26] = 1;*/
        
        Datas[1][14] = 1;
        Datas[2][14] = 1;
        Datas[3][14] = 1;
        
        Datas[1][15] = 1;
        Datas[2][15] = 2;
        Datas[3][15] = 1;

        
        // Define corners
        Corner[SRL_TopRight].x = Size;
        Corner[SRL_TopRight].z = Size;
        
        Corner[SRL_BottomRight].x = Size;
        Corner[SRL_BottomRight].z = 0;
        
        Corner[SRL_TopLeft].x = 0;
        Corner[SRL_TopLeft].z = Size;
        
        Corner[SRL_BottomLeft].x = 0;
        Corner[SRL_BottomLeft].z = 0;
        
        // Define sides
        Side[SRL_Top].x = Size/2;
        Side[SRL_Top].z = Size;
        
        Side[SRL_Left].x = 0;
        Side[SRL_Left].z = Size/2;
                
        Side[SRL_Right].x = Size;
        Side[SRL_Right].z = Size/2;
        
        Side[SRL_Bottom].x = Size/2;
        Side[SRL_Bottom].z = 0;
        
        
    }
    
    void TerrainLOD::Init() {
        for(int i = 0; i < 4; i++) {
            float Diff = Differance(i);
            if( Diff != 0 && Diff < HauteurMax )
                CreateChild(i);
        }
    }
    
};
